package tracker;
/****************************************************************************************************	
* 																									*
* TrackerSerialPort in a basic interface to access RS-232 ports with auto detect from Polhemus 		*
* trackers. 																						*
* 																									*
* Ross Smith Jan 2005																				*
* 																									*	
****************************************************************************************************/


import java.awt.event.KeyEvent;
import java.io.*;
import java.util.*;
import javax.comm.*;


public class TrackerSerialPort implements SerialPortEventListener {

	/*Enable/Disable debugging output*/
	private boolean DEBUG = false;
	
	private javax.comm.SerialPort mTrackerSerialPort = null;
	private OutputStream mOutputStream = null;
	private InputStream mInputStream = null;
	private TrackerPolhemus tracker = null;
	
	/*The serial port is a thread which continualy reads and adds data to this list,
	 this allow us to thow out unused data if other processes are using the CPU for extended
	 periods of time*/
    private List list = Collections.synchronizedList(new LinkedList());
    
	private int DROP_LENGTH;
	private int messageSize;// = 48;
	
	/*Open a device with explicit name, not used with the VR Demo world*/
	public TrackerSerialPort(String deviceName, int baudrate, int dataBits, int stopBits, int parity) {
		open(deviceName, baudrate, dataBits, stopBits, parity);
	}

	/*Search for a polhemus tracking device, note this does not traverse all the baud rates
	  as it takes too long so we tell it what speed to connect at*/
	public TrackerSerialPort() {
		/*Use some fixed numbers to speed things up*/
		int baudrate = 115200;
		int dataBits = javax.comm.SerialPort.DATABITS_8;
		int stopBits = javax.comm.SerialPort.STOPBITS_1;
		int parity = javax.comm.SerialPort.PARITY_NONE;
		
		String deviceName = null;
		
		
		/*Enumerate over all the ports attached to the hardware*/
		for (Enumeration e = CommPortIdentifier.getPortIdentifiers(); e.hasMoreElements() ;) {
	
			CommPortIdentifier comm = (CommPortIdentifier)e.nextElement();
			/*If the port is a serial port check for polhemus tracker*/
			
			if(DEBUG)
				System.out.println("Serching Commport -> " + comm.getName());
			
			if (comm.getPortType() == CommPortIdentifier.PORT_SERIAL){

				/*Open the port*/
				open(comm.getName(), baudrate, dataBits, stopBits, parity);
				
				
				write("\r", 1); /*Flush any old commands*/
				write("c", 100); /* Turn off the continuous mode if it is on */
				write("\r", 100); /* Paranoid flush any old commands */
				
				flush(); /*Now flush the list*/
				try{Thread.sleep(5);}catch(Exception e1){;} 	/*Wait a bit*/
				
				/*Ask for a system status record*/	
				write("S", 0);
				
				long startTime = System.currentTimeMillis();
				long timeOut = 500;
				String response = "";
				
				/*Check for a response for .5s*/
				while (System.currentTimeMillis() < (startTime + timeOut))
					response = response.concat(readTimed(1, 50));
				
				response = response.trim(); /*Remove the leading and trailing whitespaces*/
				
				/*Check for a valid polhemus tracker response*/
				if( response.length() > 6){
					
					/*Check if the Tracker is a fastrak*/
					if(response.substring(2,6).compareTo("S3F2") == 0){
						System.out.print("[Found Fastrak] ");
						System.out.println(comm.getName() + " response = " + response);
						deviceName = "[Fastrak]";
						
						/*Now create the appropriate tracking device*/
						tracker = new TrackerFastrak();
						break;
					}
					/*Check if the tracker is a Isotrsk*/
					else if (response.substring(2,6).compareTo("S208") == 0){
						System.out.print("[Found Isotrak II] ");
						System.out.println(comm.getName() + " response = " + response);
						deviceName = "[Isotrak II]";
						
						/*Now create the appropriate tracking device*/
						tracker = new TrackerIsotrakII();
						break;
					}
					/*Ignore the unknown device and close the port*/
					else {
						System.out.print("[Unknown device] ");
						System.out.println(comm.getName() + " response = " + response);
						close();
					}
				}
				else{
					close();
				}
			}
			else{
				/*Some other kind of port not a polhemus tracker*/
			}
		}/*End enumeration*/
		if(deviceName == null){
			System.out.println("Unable to find a tracking device");
			System.exit(1);
		}
		else{
			/*Set the message length*/
			this.messageSize = tracker.getMessageLength();
			/*Set up the length at which we decide to drop packets from the buffer*/
			DROP_LENGTH = this.messageSize * 20;
		}
	}
	
	public void flush(){
		/*Make sure all continious modes are turned off before we call this otherwise it will be really slow.*/
        while (list.size()>0){
        	list.remove(0);        		
        }     
        try{
        	this.mOutputStream.flush();
        	this.mInputStream.reset();
        }
        catch (Exception e){}
	}
	
	/*Return the handle to the tracker created*/
	public TrackerPolhemus getPolhemusTracker(){
		return this.tracker;
	}

	/*Open the serial device*/
	private boolean open(String deviceName, int baudrate, int dataBits, int stopBits, int parity) {
		try {
			CommPortIdentifier port = (CommPortIdentifier)CommPortIdentifier.getPortIdentifier(deviceName);
			
			/*If the port is not available.*/
			if(port == null)
				return false;

			mTrackerSerialPort = (javax.comm.SerialPort)port.open("VR Demo World", 2000);
			mTrackerSerialPort.setSerialPortParams(baudrate, dataBits, stopBits, parity);
			mTrackerSerialPort.addEventListener(this);		
			mTrackerSerialPort.notifyOnDataAvailable(true);
			mTrackerSerialPort.setFlowControlMode(javax.comm.SerialPort.FLOWCONTROL_NONE);     
//			mTrackerSerialPort.enableReceiveThreshold(100);
			mTrackerSerialPort.enableReceiveTimeout(10);
			mTrackerSerialPort.setInputBufferSize(1000);
			mTrackerSerialPort.setOutputBufferSize(10);
			mOutputStream = mTrackerSerialPort.getOutputStream();
			mInputStream = mTrackerSerialPort.getInputStream();
			
			return true;
		} catch (Exception e) {
			System.out.println(e);
			return false;
		}
	}

	public void close() {
		try {
			if (mOutputStream != null)
				mOutputStream.close();
			if (mInputStream != null)
				mInputStream.close();
			if (mTrackerSerialPort != null);
				mTrackerSerialPort.close();
		} catch (IOException e) {};
	}
	

	public void write(String value, long wait) {
		if (mOutputStream == null)
			return;
		try {
			mOutputStream.write(value.getBytes(), 0, value.length());
			mOutputStream.flush();
			if (wait != 0)
				Thread.sleep(wait);
		} catch (IOException e) {
		} catch (InterruptedException e){
		};
	}
	
	public void write(String value) {
		write(value, 0);
	}	
	
	public void write(byte value[]) {
		if (mOutputStream == null)
			return;
		try {
			mOutputStream.write(value);
			mOutputStream.flush();
		} catch (IOException e) {};
	}
	
	
	public synchronized String read(int msgLength, long timeOut) {
		/*Allow a buffer of double the expected size to allow for errors*/
    	if(list.size() <= this.tracker.getNumSensors()*this.tracker.getMessageLength()){
    		return null;
    	}
    	
		byte[] bbb = new byte[msgLength*this.tracker.getNumSensors()];
    	int count =0;
    	int foundCRs = 0;
    	boolean finished = false;
    	Integer CR = new Integer(KeyEvent.VK_ENTER);
    	
    	while(!finished){
    		if(list.size()>2){

		    	if(!(list.get(0).equals( new Byte(CR.toString())))){
		    		Byte bb = (Byte)list.remove(0);
		    		bbb[count++] = bb.byteValue();
		    	}
		    	else{
		    		foundCRs++;
		    		list.remove(0);
		    		if(foundCRs==this.tracker.getNumSensors())
		    			finished = true;
		    	}
	    	
    		}
    		//else
    		//	try{Thread.sleep(1);}catch(Exception e){}
    	}
    	return new String( bbb);
	}

	public String read(int ndata) {
		return read(ndata, 3000);
	}
	
	public int nToRead() {
		if (mInputStream == null)
			return 0;
		int nData;
		try{
			nData = mInputStream.available();
		} catch (IOException e) {
			nData = -1;
		};
		return nData;
	}

	public void waitData(int nWaitData) {
		int nData = 0;
		while (true) {
			nData = nToRead();
			if (nWaitData <= nData)
				break;
			waitTime(100);
		}
	}

	public void waitTime(long millis) {
		try {
			Thread.sleep(millis);
		}
		catch (InterruptedException ie) {}
	}

	public int available() {
		int numAvailable = 0;
		try {
			numAvailable = mInputStream.available();
		}
		catch(IOException e) {numAvailable = -1;}
		return numAvailable;
	}

	public void skip(long bytes) {
		try {
			mInputStream.skip(bytes);
		}
		catch(IOException e) {}
	}
	
	// this is the method that must be included to satisfy the interface, basically it
	// is a callback method that gets notified everytime there is new data on the serial 
	// port.
    public void serialEvent(SerialPortEvent event) {
        switch(event.getEventType()) {
        	
        	case SerialPortEvent.DATA_AVAILABLE:
  
        		/*Remove items from the list if we havent used them in time*/
            	if ( list.size() > DROP_LENGTH  - this.messageSize){
            		synchronized(list){
/*Fix Me remove the 48 here*/
						int dropLength = 5;
						if(this.tracker != null)
							dropLength = this.tracker.getMessageLength();
        	    		for ( int i = 0; i < DROP_LENGTH - dropLength; i++ )
        	    			list.remove(0);
            		}
            		if(DEBUG)
            			System.out.println ( "Dropping Packets from list");
            	}
            	
            	/*Add data to the list*/
				try {
					byte buffer[] = new byte[1024];
					int result = mInputStream.read( buffer, 0, mInputStream.available());
					//System.out.println(result);
					synchronized(list){
						for ( int i = 0; i < result; i++ )
							list.add ( new Byte( buffer[i] ));
					}
						
				}catch ( IOException e ){
					e.printStackTrace();
				}  
		
				break;
        }
    }

	public String readTimed(int ndata, long timeout){
		
    	int msgLength = ndata;
    	int offset = 0;
    	byte[] readBuffer = new byte[msgLength];
    
		long startTime = System.currentTimeMillis();
		
        while ( msgLength > 0 && System.currentTimeMillis() < (startTime+timeout) ){
        	/*Flush the buffer */
			if ( list.size() > 0){
        		Byte b = (Byte)list.remove(0);        		
        		readBuffer[offset++] = b.byteValue();
        		msgLength--;
        	}
        	else {
        		try {
        			Thread.sleep(1);
        		}catch ( InterruptedException e ){
        			e.printStackTrace();
        		}
        	}
        }        
        return new String( readBuffer );
	}
    
	public byte[] readBytes(int ndata){   	
		int msgLength = ndata;
		int offset = 0;
		byte[] readBuffer = new byte[msgLength];
    	
		while ( msgLength > 0 ){
			if ( list.size() > 0){
				Byte b = (Byte)list.remove(0);        		
				readBuffer[offset++] = b.byteValue();
				msgLength--;
			}
			else {
				try {
					Thread.sleep(1);
				}catch ( InterruptedException e ){
					e.printStackTrace();
				}
			}
		}        
		return readBuffer;
	}
	

    // This method only needs to be called if the data gets out of sync - which theoretically shouldn't happen except 
    // at the start.
    protected void realignBuffer(int dropBytes){
    	//System.out.println("Dropping some data to realign the buffer");
    	
    	// while we are doing this for loop we don't want anyone else messing with the list so we make it synchronized.
    	synchronized(list){
    		if(list.size()>dropBytes){
    			for(int i=0;i<=dropBytes;i++)
    				list.remove(0);
    		}
    	}
        try {
        	Thread.sleep(1);
        }catch ( InterruptedException e ){
        	e.printStackTrace();
        }
    }
    
    public int getBufferSize(){
    	return list.size();
    }
}
